/*
   This example code is in the Public Domain (or CC0 licensed, at your option.)

   Unless required by applicable law or agreed to in writing, this
   software is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR
   CONDITIONS OF ANY KIND, either express or implied.
*/

#include <stdio.h>
#include <string.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/event_groups.h"
#include "esp_log.h"
#include "nvs_flash.h"

#include "driver/gpio.h"
#include "driver/spi_master.h"

#include "feather_esp.h"
#include "mpu9250.h"

#ifdef USE_MPU9250
spi_device_handle_t mpuxx_spi;

#ifdef USE_16BIT_SPI
uint8_t spi_write_byte(spi_device_handle_t spi,uint8_t regAddr,uint8_t data)
{
    spi_transaction_t t;
    memset(&t, 0, sizeof(t));
    t.flags = 0;
    t.cmd = 0;
    t.addr = regAddr & 0x7F;
    t.length = 8;
    t.rxlength = 0;
    t.user = NULL;
    t.tx_buffer = &data;
    t.rx_buffer = NULL;
    esp_err_t err = spi_device_transmit(spi, &t);
    return err;
}

uint8_t spi_read_byte(spi_device_handle_t spi,uint8_t regAddr)
{
    uint8_t data = 0;;
    spi_transaction_t t;
    memset(&t, 0, sizeof(t));
    t.flags = 0;
    t.cmd = 0;
    t.addr = regAddr | 0x80;
    t.length = 8;
    t.rxlength = 8;
    t.user = NULL;
    t.tx_buffer = NULL;
    t.rx_buffer = &data;
    spi_device_transmit(spi, &t);    
    return data;
}
#endif

uint8_t spi_rw_byte(spi_device_handle_t spi,uint8_t data)
{
    spi_transaction_t t;

    if(data != 0xFF)
    {
        memset(&t, 0, sizeof(t));
        t.length = 8;
        t.tx_buffer = &data;
        esp_err_t ret = spi_device_transmit(spi, &t);
        assert(ret == ESP_OK);
    }
    else
    {
        memset(&t, 0, sizeof(t));
        t.rxlength = 8;
        t.rx_buffer = &data;
        esp_err_t ret = spi_device_transmit(spi, &t);    
        assert(ret == ESP_OK);
    }
    
    return data;
}

void spi_init(void)
{
    esp_err_t ret;
    spi_bus_config_t buscfg={
        .miso_io_num = MPU_MISO_PIN_NUM,
        .mosi_io_num = MPU_MOSI_PIN_NUM,
        .sclk_io_num = MPU_CLK_PIN_NUM,
        .quadwp_io_num=-1,
        .quadhd_io_num=-1,
        .max_transfer_sz=64,
    };
    spi_device_interface_config_t devcfg={
        .clock_speed_hz=1*1000*1000,            //Clock out at 1 MHz
        .mode=0,                                //SPI mode 0
        .spics_io_num=-1,//MPU_CS_PIN_NUM,           //CS pin
        .queue_size=1,                          //We want to be able to queue 7 transactions at a time
        .command_bits=0,
#ifdef USE_16BIT_SPI
        .address_bits=8,                        //全双工模式时可以 设为 8 个地址位，数据可以和地址一起发送组成16位模式
        .flags=0,
#else
        .address_bits=0,
        .flags = SPI_DEVICE_HALFDUPLEX,         //半双工模式
#endif
        .dummy_bits=0,
        .duty_cycle_pos=128,
        .cs_ena_pretrans = 0,
        .cs_ena_posttrans = 0,
        .pre_cb = NULL,
        .post_cb = NULL,
    };
    gpio_pad_select_gpio(MPU_CS_PIN_NUM);
    gpio_set_direction(MPU_CS_PIN_NUM, GPIO_MODE_OUTPUT);
    gpio_set_level(MPU_CS_PIN_NUM, 0);
    //Initialize the SPI bus, No DMA
    ret=spi_bus_initialize(HSPI_HOST, &buscfg, 0);
    ESP_ERROR_CHECK(ret);
    //Attach the dev to the SPI bus
    ret=spi_bus_add_device(HSPI_HOST, &devcfg, &mpuxx_spi);
    ESP_ERROR_CHECK(ret);
}

uint8_t mpuxx_getid(spi_device_handle_t spi)
{
    uint8_t id = 0;
    //get_id cmd
    gpio_set_level(MPU_CS_PIN_NUM, 0);
#ifdef USE_16BIT_SPI
    id = spi_read_byte(spi,0x75);
#else
    spi_rw_byte(spi,0x80 | 0x75);
    id = spi_rw_byte(spi,0xFF);
#endif
    gpio_set_level(MPU_CS_PIN_NUM, 1);
    return id;
}

void mpuxx_task(void *pvParameter)
{
    spi_init();
#ifdef USE_MPU9250_DMP
    if(!mpu9250_dmp_init())
        printf("mpu9250 dmp init success!\r\n");
#else
    uint8_t id = 0;
    int16_t a[3],g[3],m[3];
    mpu9250_init();
#endif
    while(1)
    {
#ifndef USE_MPU9250_DMP
        id = mpuxx_getid(mpuxx_spi);
        mpu9250_get_9axis_data(a,g,m); 
        printf("GET MPU9250 ID: 0x%02X: %d,%d,%d %d,%d,%d %d,%d,%d\r\n",id,a[0],a[1],a[2],g[0],g[1],g[2],m[0],m[1],m[2]);
#endif
        vTaskDelay(1000 / portTICK_PERIOD_MS);
    }
}
#endif

